/*
 * Copyright (C) 2011 Google Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */

package org.ros.android.view.visualization.layer;

import org.ros.android.view.visualization.VisualizationView;
import org.ros.android.view.visualization.shape.GoalShape;
import org.ros.android.view.visualization.shape.Shape;
import org.ros.namespace.GraphName;
import org.ros.node.ConnectedNode;
import org.ros.rosjava_geometry.FrameTransform;
import org.ros.rosjava_geometry.Transform;

import java.util.List;

import javax.microedition.khronos.opengles.GL10;

import geometry_msgs.Pose;
import geometry_msgs.PoseWithCovarianceStamped;
import xia.Bean.Bean_from_slam_mark_15;

/**
 * @author moesenle@google.com (Lorenz Moesenlechner)
 */
public class MyPointShowLayer extends SubscriberLayer<geometry_msgs.PoseStamped> implements
    TfLayer {

  private final GraphName targetFrame;

  //private Shape shape;
  private boolean ready;
  VisualizationView my_view;
  //ArrayList<Shape> mapPoints;
  public MyPointShowLayer(VisualizationView view,String topic) {
    this(GraphName.of(topic));
    //shape = new GoalShape();
    my_view=view;
    ready = false;

  }
  List<Bean_from_slam_mark_15.MarkCoordinateBean> my_mark_coordinate;
  Pose tmpPose;
  public void add_point(List<Bean_from_slam_mark_15.MarkCoordinateBean> mark_coordinate, Pose tmpPosexx)
  {
    ready = false;
    my_mark_coordinate=mark_coordinate;
    tmpPose=tmpPosexx;
    ready = true;
  }



  public MyPointShowLayer(GraphName topic) {
    super(topic, geometry_msgs.PoseStamped._TYPE);
    targetFrame = GraphName.of("map");
    ready = false;
  }

  @Override
  public void draw(VisualizationView view, GL10 gl) {
    if (ready)
    {
      //shape.draw(view, gl);
      //L.e("----------draw-----------coordinate_x 000---mapPoints.size()="+mapPoints.size());
      for(int i=0;i<my_mark_coordinate.size();i++)
      {

        Float coordinate_x = my_mark_coordinate.get(i).coordinate.get(0);
        Float coordinate_y = my_mark_coordinate.get(i).coordinate.get(1);
        Float coordinate_z = my_mark_coordinate.get(i).coordinate.get(2);

        Float orientation_x = my_mark_coordinate.get(i).coordinate.get(3);
        Float orientation_y = my_mark_coordinate.get(i).coordinate.get(4);
        Float orientation_z = my_mark_coordinate.get(i).coordinate.get(5);
        Float orientation_w = my_mark_coordinate.get(i).coordinate.get(6);

        tmpPose.getPosition().setX(coordinate_x);
        tmpPose.getPosition().setY(coordinate_y);
        tmpPose.getPosition().setZ(coordinate_z);

        tmpPose.getOrientation().setX(orientation_x);
        tmpPose.getOrientation().setY(orientation_y);
        tmpPose.getOrientation().setZ(orientation_z);
        tmpPose.getOrientation().setW(orientation_w);

        Transform poseTransform = Transform.fromPoseMessage(tmpPose);

        Shape tmp=new GoalShape();
        tmp.setTransform(poseTransform);
        tmp.draw(view, gl);
      }
    }
  }


  public void setMyPointPost( PoseWithCovarianceStamped mypost_message) {
    GraphName source = GraphName.of(mypost_message.getHeader().getFrameId());
    FrameTransform frameTransform = my_view.getFrameTransformTree().transform(source, targetFrame);
    if (frameTransform != null) {
        ready = true;
        Transform poseTransform = Transform.fromPoseMessage(mypost_message.getPose().getPose());
        //shape.setTransform(frameTransform.getTransform().multiply(poseTransform));
    }
  }



  @Override
  public void onStart(final VisualizationView view, ConnectedNode connectedNode) {
    super.onStart(view, connectedNode);
  }

  @Override
  public GraphName getFrame() {
    return targetFrame;
  }
}